#include "ds3d.h"

#include <nds/bios.h>

static uint32 next_texture_block;
static uint32 next_palette_block;

void DSInit3D()
{
	while(GFX_STATUS&(1<<27)); // wait till gfx engine is not busy

	GFX_STATUS|=(1<<29); // Clear the FIFO
	DSResetMatrixStack(); // Clear overflows from list memory
	DSFlush(0); // prime the vertex/polygon buffers
	GFX_CONTROL=0; // reset the control bits
	DSClearParams(0,0,0,31,0); // reset the rear-plane(a.k.a. clear color) to black, ID=0, and opaque
	DSClearDepth(DS_MAX_DEPTH);// reset the depth to its max

	GFX_TEX_FORMAT=0;
	GFX_POLY_FORMAT=0;

	DSFreeAllTextures();
//	DSFreeAllPalettes();
}



void DSRotatef32i(int angle,int32 x,int32 y,int32 z)
{
	int32 axis[3];
	int32 sine=isin(angle);
	int32 cosine=icos(angle);
	int32 one_minus_cosine=DSf32(1)-cosine;

	axis[0]=x;
	axis[1]=y;
	axis[2]=z;
	normalizef32(axis);

	MATRIX_MULT3x3=cosine + mulf32(one_minus_cosine, mulf32(axis[0], axis[0]));
	MATRIX_MULT3x3=mulf32(one_minus_cosine, mulf32(axis[0], axis[1])) - mulf32(axis[2], sine);
	MATRIX_MULT3x3=mulf32(mulf32(one_minus_cosine, axis[0]), axis[2]) + mulf32(axis[1], sine);

	MATRIX_MULT3x3=mulf32(mulf32(one_minus_cosine, axis[0]),  axis[1]) + mulf32(axis[2], sine);
	MATRIX_MULT3x3=cosine + mulf32(mulf32(one_minus_cosine, axis[1]), axis[1]);
	MATRIX_MULT3x3=mulf32(mulf32(one_minus_cosine, axis[1]), axis[2]) - mulf32(axis[0], sine);

	MATRIX_MULT3x3=mulf32(mulf32(one_minus_cosine, axis[0]), axis[2]) - mulf32(axis[1], sine);
	MATRIX_MULT3x3=mulf32(mulf32(one_minus_cosine, axis[1]), axis[2]) + mulf32(axis[0], sine);
	MATRIX_MULT3x3=cosine + mulf32(mulf32(one_minus_cosine, axis[2]), axis[2]);
}



/*
//---------------------------------------------------------------------------------
// glColorTable establishes the location of the current palette.
//	Roughly follows glColorTableEXT. Association of palettes with 
//	named textures is left to the application. 
//---------------------------------------------------------------------------------
void glColorTable( uint8 format, uint32 addr ) {
//---------------------------------------------------------------------------------
	GFX_PAL_FORMAT = addr>>(4-(format==GL_RGB4));
}
                     

//---------------------------------------------------------------------------------
inline uint32 alignVal( uint32 val, uint32 to ) {
	return (val & (to-1))? (val & ~(to-1)) + to : val;
}

//---------------------------------------------------------------------------------
int getNextPaletteSlot(u16 count, uint8 format) {
//---------------------------------------------------------------------------------
	// ensure the result aligns on a palette block for this format
	uint32 result = alignVal(glGlob->nextPBlock, 1<<(4-(format==GL_RGB4)));

	// convert count to bytes and align to next (smallest format) palette block
	count = alignVal( count<<1, 1<<3 ); 

	// ensure that end is within palette video mem
	if( result+count > 0x10000 )   // VRAM_F - VRAM_E
		return -1;

	glGlob->nextPBlock = result+count;
	return (int)result;
} 

//---------------------------------------------------------------------------------
void glTexLoadPal(const u16* pal, u16 count, u32 addr) {
//---------------------------------------------------------------------------------
	vramSetBankE(VRAM_E_LCD);
	swiCopy( pal, &VRAM_E[addr>>1] , count / 2 | COPY_MODE_WORD);
	vramSetBankE(VRAM_E_TEX_PALETTE);
}

//---------------------------------------------------------------------------------
int gluTexLoadPal(const u16* pal, u16 count, uint8 format) {
//---------------------------------------------------------------------------------
	int addr = getNextPaletteSlot(count, format);
	if( addr>=0 )
		glTexLoadPal(pal, count, (u32) addr);

	return addr;
}


*/



uint32 DSTextureSize(uint32 flags)
{
	uint32 pixels=1<<(((flags>>20)&7)+((flags>>23)&7)+6);

	switch(flags&DS_TEX_FORMAT_MASK)
	{
		case DS_TEX_FORMAT_A3P5:
		case DS_TEX_FORMAT_PAL8:
		case DS_TEX_FORMAT_A5P3:
			return pixels;

		case DS_TEX_FORMAT_NONE:
		case DS_TEX_FORMAT_COMPRESSED:
			return 0;

		case DS_TEX_FORMAT_RGB:
			return pixels*2;

		case DS_TEX_FORMAT_PAL2:
			return pixels/4;

		case DS_TEX_FORMAT_PAL4:
			return pixels/2;
	}
	return 0;
}

uint16 *DSTextureAddress(uint32 texture)
{
	return (uint16 *)(((texture&0xffff)<<3)|0x6800000);
}

static int VRAMAddressIsTextureBank(uint32 addr)
{
	if((uint16 *)addr<VRAM_A) return 0;
	else if((uint16 *)addr<VRAM_B) return (VRAM_A_CR&3)==((VRAM_A_TEXTURE)&3);
	else if((uint16 *)addr<VRAM_C) return (VRAM_B_CR&3)==((VRAM_B_TEXTURE)&3);
	else if((uint16 *)addr<VRAM_D) return (VRAM_C_CR&3)==((VRAM_C_TEXTURE)&3);
	else if((uint16 *)addr<VRAM_E) return (VRAM_D_CR&3)==((VRAM_D_TEXTURE)&3);
	else return 0;
}

uint32 DSAllocTexture(uint32 flags)
{
	uint32 size=DSTextureSize(flags);
	uint32 addr=next_texture_block;

	next_texture_block+=size;

	// Bug: does not handle non-contiguous texture memory gracefully with large allocations
	while(!VRAMAddressIsTextureBank(next_texture_block-1) && next_texture_block<=(uint32)VRAM_E)
	{
		addr=next_texture_block=(next_texture_block&~0x1ffff)+0x20000;
		next_texture_block+=size;
	}

	if(next_texture_block>(uint32)VRAM_E) return DS_INVALID_TEXTURE;

	return flags|((addr>>3)&0xffff);
}

void DSFreeAllTextures()
{
	next_texture_block=(uint32)VRAM_A;
}

void DSCopyTexture(uint32 texture,void *data)
{
	uint32 size=DSTextureSize(texture);
	void *dest=DSTextureAddress(texture);

	uint32 vramtmp=vramSetMainBanks(VRAM_A_LCD,VRAM_B_LCD,VRAM_C_LCD,VRAM_D_LCD);
	swiCopy(data,dest,size/4|COPY_MODE_WORD);
	vramRestoreMainBanks(vramtmp);
}

uint32 DSAllocAndCopyTexture(uint32 flags,void *data)
{
	uint32 tex=DSAllocTexture(flags);
	if(tex==DS_INVALID_TEXTURE) return DS_INVALID_TEXTURE;
	DSCopyTexture(tex,data);
	return tex;
}

uint32 DSMakeColorTexture(uint32 color)
{
	uint32 texture=DSAllocTexture(DS_TEX_SIZE_T_8|DS_TEX_SIZE_S_8|DS_TEX_WRAP_S|DS_TEX_WRAP_T);
	uint16 *dest=DSTextureAddress(texture);
	uint32 size=DSTextureSize(texture)/2;
	for(int i=0;i<size;i++) *dest++=color|0x8000;
	return texture;
}

uint32 DSMakeWhiteTexture()
{
	return 0;
	return DSMakeColorTexture(0x7fff);
}



void DSSetFogf32(uint8 r,uint8 g,uint8 b,uint8 a,uint32 start,uint32 end,uint32 near,uint32 far)
{
	uint8 control=GFX_CONTROL&~0xf00;

	uint32 startdepth=mulf32(0x7fff,divf32(mulf32(near,far),(mulf32(start,(far-near))-far)));
	uint32 enddepth=mulf32(0x7fff,divf32(mulf32(near,far),(mulf32(end,(far-near))-far)));

	uint32 diff=enddepth-startdepth-1;
	int log=0;
	while(diff>>=1) log++;

	int shift=14-log;
	if(shift>10) shift=10;

	GFX_CONTROL=control|DS_FOG_SHIFT(shift);

	GFX_FOG_COLOR=((a&0x1f)<<16)|DSPackRGB5(r,g,b);
	GFX_FOG_OFFSET=startdepth;

	for(int i=0;i<32;i++)
	{
		uint32 depth=i*(0x400>>shift);
		if(depth>=enddepth-startdepth) GFX_FOG_TABLE[i]=0x7f;
		else GFX_FOG_TABLE[i]=mulf32(0x7f,divf32(depth,enddepth-startdepth));
	}
}

void DSSetFogf(uint8 r,uint8 g,uint8 b,uint8 a,float start,float end,float near,float far)
{ DSSetFogf32(r,g,b,a,DSf32(start),DSf32(end),DSf32(near),DSf32(far)); }
